#include "pathfinder.h"

#include <map>
#include <queue>
#include <list>
using std::priority_queue;
using std::list;

pathfinder::pathfinder(uicoord o,uicoord d):
origin( o ), destination( d ){}

class pf_node{
    public:
        uicoord position;
        unsigned int heritage;
        unsigned int heuristique;
        unsigned int score;
        pf_node* parent;

        pf_node(): heritage(0),heuristique(0),score(0){}
        pf_node(uicoord p,unsigned int s,unsigned int h): position( p ),heritage(0),heuristique(h),score( s ){}

};

bool pf_node_cmp(const pf_node &a,const pf_node &b){
    return a.score<b.score;
}

struct uicoordcomp {
    bool operator() (const uicoord& a, const uicoord& b) const
    {   if(a.x<b.x){return true;}else if(a.x>b.x){return false;}else{return a.y<b.y;}}
};


#define MAX_POSITIONS 8
static bool position(int i,map* mymap,pf_node* parent,pf_node* children){
    *children = *parent;
    children->parent = parent;
    if( i < 4 ){
        children->heritage += 10;
        switch(i){
            case 0: children->position.x++; break;
            case 1: children->position.x--; break;
            case 2: children->position.y++; break;
            case 3: children->position.y--; break;
        }
        if( !map::isWalkable( mymap->elem(children->position) ) ) return false;
    }else{
        children->heritage += 14;
        switch(i){
            case 4:{
                    children->position.x++;
                    children->position.y++;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, -1, 0 ) ) ) ) return false;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, 0, -1 ) ) ) ) return false;
                    break;
                }
            case 5:{
                    children->position.x--;
                    children->position.y--;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, +1, 0 ) ) ) ) return false;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, 0, +1 ) ) ) ) return false;
                    break;
                }
            case 6:{
                    children->position.x--;
                    children->position.y++;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, +1, 0 ) ) ) ) return false;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, 0, -1 ) ) ) ) return false;
                    break;
                }
            case 7:{
                    children->position.x++;
                    children->position.y--;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, -1, 0 ) ) ) ) return false;
                    if( !map::isWalkable( mymap->elem( map::translate( children->position, 0, +1 ) ) ) ) return false;
                    break;
                }
        }
        if( !map::isWalkable( mymap->elem(children->position) ) ) return false;
    }
    return true;
}

static unsigned int heuristique(uicoord a,uicoord b){
    int dx = a.x - b.x;
    int dy = a.y - b.y;
    if( dx < 0 ) dx = -dx;
    if( dy < 0 ) dy = -dy;
    return (dx + dy)*10;
}

bool pathfinder::compute(map* mymap,uicoord* stPoint){
    list<pf_node>   openedList;
    std::map<uicoord,pf_node,uicoordcomp>           closedList;
    data.clear();

    if( stPoint ){
        origin = *stPoint;
    }

    openedList.push_back( pf_node(origin,0,heuristique( origin, destination )) );
    bool done = false;
    while( !openedList.empty() && !done ){
        pf_node stack = openedList.front();
        pf_node* ptr;
        openedList.pop_front();
        ptr = &closedList[stack.position];
        *ptr = stack;
        if( stack.position.x == destination.x && stack.position.y == destination.y ){
            done = true;
            continue;
        }
        for( int i = 0; i < MAX_POSITIONS; i++ ){
            if( !position(i,mymap,ptr,&stack) ) continue;
            if( closedList.find(stack.position) != closedList.end() ) continue;
            stack.heuristique = heuristique(stack.position,destination);
            stack.score = stack.heuristique + stack.heritage;
            list<pf_node>::iterator needle;
            for( needle = openedList.begin(); needle != openedList.end(); needle++ ){
                if( (*needle).position.x == stack.position.x && (*needle).position.y == stack.position.y ) break;
            }
            if( needle != openedList.end() ){
                if( (*needle).score > stack.score ){
                    (*needle).score = stack.score;
                    (*needle).heritage = stack.heritage;
                    (*needle).parent = stack.parent;
                }
            }
            else openedList.push_back( stack );
        }
        openedList.sort(pf_node_cmp);
    }

    if( !done ) return false;
    pf_node* result = &(closedList[destination]);
    do{
        data.push_back(result->position);
    }while( (result = result->parent) );
    if( stPoint ){
        data.push_back(*stPoint);
    }

    return true;
}

bool pathfinder::getNextPoint(uicoord* out){
    if( data.empty() ) return false;
    *out = data.back();
    data.pop_back();
    return true;
}

